import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
    # 获取当前package的目录，并以此得到地图文件的文件名
    package_dir = get_package_share_directory('simulator')
    yaml_filename = os.path.join(package_dir, 'maps', 'MG_map.yaml')
    # 设置rviz的加载配置文件位置
    rviz_config = os.path.join(package_dir, 'rviz', '2d.rviz')
    config_lidar = os.path.join(package_dir,'params','lidar.yaml')
    config_odom_tf = os.path.join(package_dir,'params','odom_tf.yaml')
    # 定义启动参数
    #min_angle = LaunchConfiguration('min_angle', default='-1.57')
    #max_angle = LaunchConfiguration('max_angle', default='1.57')
    #min_distance = LaunchConfiguration('min_distance', default='0.05')
    #max_distance = LaunchConfiguration('max_distance', default='80.00')
    #size = LaunchConfiguration('size', default='400')
    #rate = LaunchConfiguration('rate', default='10')
    #noise = LaunchConfiguration('noise', default='0.001')
    #stage_map_topic = LaunchConfiguration('stage_map_topic', default='map')
    #global_frame = LaunchConfiguration('global_frame', default='map')
    #lidar_frame = LaunchConfiguration('lidar_frame', default='laser_link')
    remappings = [('/tf', 'tf'),
                  ('/tf_static', 'tf_static')]
    urdf = os.path.join(package_dir, 'urdf', 'robot_mini.urdf')
    with open(urdf, 'r') as infp:
        robot_description = infp.read()
    
    # 设置需要被lifecycle manager管理的lifecycle node
    lifecycle_nodes = ['map_server']
    return LaunchDescription([
        # 启动urdf
        Node(
           package='robot_state_publisher',
           executable='robot_state_publisher',
           name='robot_state_publisher',
           output='screen',
           parameters=[{'use_sim_time': True,
                     'robot_description': robot_description}],
           remappings=remappings),
        # 启动仿真时间节点，在/clock话题中发布时钟
        Node(
           package='simulator',
           executable='clock_node',
           name='clock',
           output='screen'
           ),
        # rviz控件指令桥接器，用于planner server指令翻译发送
        #Node(
        #   package='simulator',
        #   executable='planner_bridge',
        #   name='planner_bridge',
        #   parameters=[{},
        #               {'planner_id': "GridBased"}],
        #   output='screen'
        #   ),   
        # rviz控件指令桥接器，用于planner server指令翻译发送
        # Node(
        #    package='simulator',
        #    executable='path_bridge',
        #    name='path_bridge',
        #    parameters=[{},
        #                {'controller_id': "FollowPath"},
        #                {'goal_checker_id': "general_goal_checker"}],
        #    output='screen'
        #    ),   
        # rviz控件指令桥接器，用于planner server指令翻译发送
        #Node(
        #   package='simulator',
        #   executable='path_pub',
        #   name='path_pub',
        #   parameters=[{},
        #               {'controller_id': "FollowPath"},
        #               {'goal_checker_id': "general_goal_checker"},
        #               {'filename': "/home/robot/map/path.txt"}],
        #   output='screen'
        #   ),        
        # 发布一个静态的坐标变换关系，用于描述map->base_link
        Node(
           package='simulator',
           executable='odom_tf',
           name='odom_tf',
           parameters=[{'use_sim_time': True},
                       config_odom_tf]),    
        # Node(
        #     package='tf2_ros',
        #     executable='static_transform_publisher',
        #     name='map_to_laser_link',
        #     arguments=[
        #         '--x', '0', '--y', '0', '--z', '0',
        #         '--yaw', '0', '--pitch', '0', '--roll', '0',
        #         '--frame-id', 'base_link', '--child-frame-id', 'laser_link'
        #     ],
        #     parameters=[{}],),
        # 打开一个用于显示的rviz2终端
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config],
            parameters=[{'use_sim_time': True}],
            ),
        # 启动map_server并加载上面变量中的map文件
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'yaml_filename': yaml_filename},
                        {'use_sim_time': True}]),
        # simulator Lidar
        Node(
            package='simulator',
            executable='simulator_lidar',
            name='simulator_lidar',
            output='screen',
            parameters=[{'use_sim_time': True},config_lidar]
        ),
        # 启动lifecycle manager节点，用于管理map_server的生命周期
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_simulator',
            output='screen',
            parameters=[{'use_sim_time': True},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}]) 
    ])
